#include "types.h"
#include "qmath.h"

#include "state.h"
#include "config.h"

#define PIDCOUNT 16

const char * names[] = {"MotorSpeed",
                        "AcroRoll","AcroPitch","AcroYaw",
                        "StabRoll","StabPitch","StabYaw",
                        "AltHold"};



void pidClearCalculated()
{
	int i;
	for (i=0;i<PIDCOUNT;i++)
	{
		STATE.pid[i].calculated = 0;
	}
}


void pidCalcSetpoint(int id)
{
    struct PidConfig_t * pid = &CONFIG.pid[id];
    if (pid->setpoint == NULL) return;

    STATE.pid[id].values.setpoint = *pid->setpoint->pointer;
    STATE.pid[id].values.setpoint += pid->setpointTranslate;
    STATE.pid[id].values.setpoint = qMul(STATE.pid[id].values.setpoint, pid->setpointScale);
    if (STATE.pid[id].values.setpoint < pid->setpointClip[0]) STATE.pid[id].values.setpoint = pid->setpointClip[0];
    if (STATE.pid[id].values.setpoint > pid->setpointClip[1]) STATE.pid[id].values.setpoint = pid->setpointClip[1];
}

void pidSwitchToMode(BYTE to, BYTE from)
{
    int i;
    for (i=0;i<PIDCOUNT;i++)
    {
        if ((CONFIG.pid[i].modeActive & to) &&
            (CONFIG.pid[i].modeActive & from) == 0)
        {
            STATE.pid[i].Isum = 0;
            STATE.pid[i].prevError = 0;
            pidCalcSetpoint(i);
            STATE.pid[i].prevSetPoint = STATE.pid[i].values.setpoint;
        }
    }
}


void pidCalculate(int id)
{
    struct PidConfig_t * pid = &CONFIG.pid[id];

    if (pid->setpoint == NULL) return;
    if (pid->feedback == NULL) return;

    if (pid->setpoint->isPIDreference != 255 && STATE.pid[pid->setpoint->isPIDreference].calculated == 0)
    {
        pidCalculate(pid->setpoint->isPIDreference);
    }

    pidCalcSetpoint(id);

	STATE.pid[id].values.feedback = *pid->feedback->pointer;
    STATE.pid[id].values.feedback += pid->feedbackTranslate;
    STATE.pid[id].values.feedback = qMul(STATE.pid[id].values.feedback, pid->feedbackScale);


	// calculate the error
	q_t error = STATE.pid[id].values.setpoint - STATE.pid[id].values.feedback;

	// integrate the error for the I-part
    q_t pidDt = Q_ONE / CONFIG.imu.freq;
    
	STATE.pid[id].Isum += qMul3(error, pid->I, pidDt);

    // integration clipping
	if (STATE.pid[id].Isum > pid->I_limit) STATE.pid[id].Isum = pid->I_limit;
	if (STATE.pid[id].Isum < -pid->I_limit) STATE.pid[id].Isum = -pid->I_limit;

	// standard P & I terms
	q_t Pterm = qMul(error, pid->P);
	q_t Iterm = STATE.pid[id].Isum;

	q_t Dterm;

	// check if Dterm uses direct value instead of delta-error
	/*if (pid->feedbackDValue != NULL)
	{
		Dterm = *pid->feedbackDValue;

		if (pid->feedbackDScale != 0) Dterm = qMul(Dterm, pid->feedbackDScale);
	}*/
	//else
	{
		Dterm = STATE.pid[id].prevError - error;

		// compensate for change in setpoint
		Dterm -= STATE.pid[id].values.setpoint - STATE.pid[id].prevSetPoint;
	}

	// standard D term calculation
	Dterm = qMul(Dterm, pid->D);
    //Dterm = qDiv(Dterm, pidDt);

	// output value!
	STATE.pid[id].values.output = Pterm + Iterm + Dterm;

    // clip output vals
    if (STATE.pid[id].values.output < pid->outputClip[0]) STATE.pid[id].values.output = pid->outputClip[0];
    if (STATE.pid[id].values.output > pid->outputClip[1]) STATE.pid[id].values.output = pid->outputClip[1];

	// store values for next run
	STATE.pid[id].prevSetPoint = STATE.pid[id].values.setpoint;
	STATE.pid[id].prevError = error;

	// we've been calculated...
	STATE.pid[id].calculated = 1;

    logStore(xTaskGetTickCount(), LOG_ID_PID_0 + id, STATE.pid[id].values);
}

void pidCalculateAll()
{
	int i,j;
	pidClearCalculated();

	for (i=0;i<PIDCOUNT;i++)
	{
		if (STATE.pid[i].calculated == 1) continue;

		if (CONFIG.pid[i].modeActive & STATE.imu.mode)
		{
			pidCalculate(i);
		}
	}

	q_t motorOutput[8];
    for (i=0;i<8;i++) motorOutput[i] = 0;

	for (i=0;i<PIDCOUNT;i++)
	{
		if (STATE.pid[i].calculated == 0) continue;

		for (j=0;j<8;j++)
		{
			motorOutput[j] += qMul(STATE.pid[i].values.output, CONFIG.pid[i].motorMix[j]);
		}
	}

	for (j=0;j<8;j++)
	{
		//if (motorOutput[j] < 0) motorOutput[j] = 0;
		//if (motorOutput[j] > 10000) motorOutput[j] = 10000;
        if (motorOutput[j] < 0) motorOutput[j] = 0;

        STATE.motors.setpoints[j].setPointRPM = motorOutput[j] / Q_ONE;
	}
}
